#include "stdafx.h"
#include "..\headers\PhysicsWorld.h"
#include "..\headers\LuaGameObject.h"
#include "..\headers\GameObjectManager.h"
#include "..\headers\PhysicsComponents.h"
#include "ext\btOgre\BtOgreExtras.h"

namespace ne{

PhysicsWorld::PhysicsWorld(void):
	m_drawDebugData(false),
	m_debugDraw(nullptr),
	m_broadphaseInterface(nullptr),
	m_collisionConfiguration(nullptr),
	m_collisionDispatcher(nullptr),
	m_dynamicWorld(nullptr),
	m_ghostPairCallback(nullptr),
	m_sequentialImpulseConstraintSolver(nullptr),
	m_gravity(btVector3(0.0f,-9.8f,0.0f)){}
PhysicsWorld::~PhysicsWorld(void){}

void PhysicsWorld::Init(){
	m_broadphaseInterface=new btDbvtBroadphase;
	m_ghostPairCallback=new btGhostPairCallback;
	m_broadphaseInterface->getOverlappingPairCache()->setInternalGhostPairCallback(m_ghostPairCallback);
	m_collisionConfiguration=new btDefaultCollisionConfiguration;
	m_collisionDispatcher=new btCollisionDispatcher(m_collisionConfiguration);
	m_dynamicWorld=new btDiscreteDynamicsWorld(m_collisionDispatcher,m_broadphaseInterface,m_sequentialImpulseConstraintSolver,m_collisionConfiguration);
	m_dynamicWorld->setGravity(m_gravity);
	m_dynamicWorld->setInternalTickCallback(GameTickCallback,static_cast<void*>(this),true);
}

void PhysicsWorld::Shut(){
	if (m_dynamicWorld){
		delete m_dynamicWorld;
		m_dynamicWorld=nullptr;
	}
	if (m_sequentialImpulseConstraintSolver){
		delete m_sequentialImpulseConstraintSolver;
		m_sequentialImpulseConstraintSolver=nullptr;
	}
	if (m_collisionConfiguration){
		delete m_collisionConfiguration;
		m_collisionConfiguration=nullptr;
	}
	if (m_collisionDispatcher){
		delete m_collisionDispatcher;
		m_collisionDispatcher=nullptr;
	}
	if (m_broadphaseInterface){
		delete m_broadphaseInterface;
		m_broadphaseInterface=nullptr;
	}
	if(m_ghostPairCallback){
		delete m_ghostPairCallback;
		m_ghostPairCallback=nullptr;
	}
}

void PhysicsWorld::SetGravity(float x, float y, float z){
	m_gravity = btVector3(x,y,z);
	m_dynamicWorld->setGravity(m_gravity);
}

void PhysicsWorld::SetGravity(const btVector3& gravity){
	m_gravity=gravity;
	m_dynamicWorld->setGravity(m_gravity);
}

void PhysicsWorld::SetGravity(const Ogre::Vector3& gravity){
	m_gravity=Convert::ToBullet(gravity);
	m_dynamicWorld->setGravity(m_gravity);
}

void PhysicsWorld::Step(GameTime& gameTime){
	float fixedTimeStep=1.0f/gameTime.FrameLimit();
	int maxSteps=2;
	while (gameTime.DeltaTime() > (float)maxSteps*fixedTimeStep){
		maxSteps++;
	}
	m_dynamicWorld->stepSimulation(gameTime.DeltaTime(),maxSteps,fixedTimeStep);
	if (m_drawDebugData&&m_debugDraw!=nullptr){
		static_cast<BtOgre::DebugDrawer*>(m_debugDraw)->step();
	}
}

void PhysicsWorld::ProcessObjectSimulationAndCollision(btScalar timeStep){
	if(m_gameObjectManager){
		m_gameObjectManager->UpdateSimulation();
	}
	CollisionList newContacts;

	int numManifolds=m_dynamicWorld->getDispatcher()->getNumManifolds();
	for (int i=0;i<numManifolds;i++){
		btPersistentManifold* contactManifold = m_dynamicWorld->getDispatcher()->getManifoldByIndexInternal(i);
		const btCollisionObject* obA = static_cast<const btCollisionObject*>(contactManifold->getBody0());
		const btCollisionObject* obB = static_cast<const btCollisionObject*>(contactManifold->getBody1());
		int numContacts = contactManifold->getNumContacts();
		for (int j=0;j<numContacts;j++){
			btManifoldPoint& pt = contactManifold->getContactPoint(j);
			if (pt.getDistance()<0.0f){
				auto it = std::find(newContacts.begin(),newContacts.end(),std::make_pair(obA,obB));
				if (it == newContacts.end()){
					newContacts.push_back(std::make_pair(obA,obB));
				}
			}
		}
	}
	if (!newContacts.empty()){
		for (unsigned int i=0;i<newContacts.size();i++){
			auto it=std::find(m_contacts.begin(),m_contacts.end(),newContacts[i]);
			if (it==m_contacts.end()){
				//signal beginContact
				GameObjectCollisionData* goA = static_cast<GameObjectCollisionData*>(newContacts[i].first->getUserPointer());
				GameObjectCollisionData* goB = static_cast<GameObjectCollisionData*>(newContacts[i].second->getUserPointer());
				static_cast<LuaGameObject*>(goA->m_gameObject)->beginContact(goB->m_gameObject);
				static_cast<LuaGameObject*>(goB->m_gameObject)->beginContact(goA->m_gameObject);
				goA->m_owner->SetInContact(true);
				goB->m_owner->SetInContact(true);
			}
			else {
				m_contacts.erase(it);
			}
		}
	}
	if (!m_contacts.empty()){
		for (unsigned int i=0;i<m_contacts.size();i++){
			//signal endContact
			GameObjectCollisionData* goA = static_cast<GameObjectCollisionData*>(m_contacts[i].first->getUserPointer());
			GameObjectCollisionData* goB = static_cast<GameObjectCollisionData*>(m_contacts[i].second->getUserPointer());
			static_cast<LuaGameObject*>(goA->m_gameObject)->endContact(goB->m_gameObject);
			static_cast<LuaGameObject*>(goB->m_gameObject)->endContact(goA->m_gameObject);
			goA->m_owner->SetInContact(false);
			goB->m_owner->SetInContact(false);
		}
		m_contacts.clear();
	}
	m_contacts=newContacts;
}

void PhysicsWorld::InitDebugDraw(Ogre::SceneManager* sceneManager){
	if (m_debugDraw==nullptr){
		m_debugDraw = new BtOgre::DebugDrawer(sceneManager->getRootSceneNode(),m_dynamicWorld);
		m_dynamicWorld->setDebugDrawer(m_debugDraw);
	}
}

void PhysicsWorld::Close(){
	if (m_debugDraw!=nullptr){
		m_dynamicWorld->setDebugDrawer(nullptr);
		delete m_debugDraw;
		m_debugDraw = nullptr;
	}
	if (!m_contacts.empty()){
		m_contacts.clear();
	}
}

void PhysicsWorld::RemoveContact(IComponentCollision* componentCollision){
	for(unsigned int i=0;i<m_contacts.size();i++){
		GameObjectCollisionData* goA = static_cast<GameObjectCollisionData*>(m_contacts[i].first->getUserPointer());
		GameObjectCollisionData* goB = static_cast<GameObjectCollisionData*>(m_contacts[i].second->getUserPointer());
		if(goA->m_owner==componentCollision){
			m_contacts.erase(m_contacts.begin()+i);
			i--;
		}
		else if(goB->m_owner==componentCollision){
			m_contacts.erase(m_contacts.begin()+i);
			i--;
		}
	}
}

void GameTickCallback(btDynamicsWorld* world, btScalar timeStep){
	PhysicsWorld* pw=static_cast<PhysicsWorld*>(world->getWorldUserInfo());
	pw->ProcessObjectSimulationAndCollision(timeStep);
}

}